import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.TachoPilot;


public class Test {
	static Motor leftMotor = Motor.B;
	static Motor rightMotor = Motor.A;
	/**
	 * @param args
	 * @throws InterruptedException 
	 */
	public static void main(String[] args) throws InterruptedException {
		leftMotor.setSpeed(180);
		rightMotor.setSpeed(180);
		TouchSensor tSensor = new TouchSensor(SensorPort.S4);
		UltrasonicSensor usSensor = new UltrasonicSensor(SensorPort.S1);
		leftMotor.forward();
		rightMotor.forward();
		long x = 0;
		while(true) {
			int usSensorWert = usSensor.getDistance();
			if(usSensorWert < 14) {
				Sound.beep();
				turn(180);
			}
			if(tSensor.isPressed()) {
				System.exit(0);
			}
			++x;
			if(x>=10) {
				LCD.drawString("US: " + usSensorWert, 0, 0);
				x = 0;
			}
			Thread.sleep(50);
		}
	
	}
	private static void turn(int degrees) throws InterruptedException {		
//		leftMotor.resetTachoCount();
//		rightMotor.resetTachoCount();
//		
		if(degrees > 0) {
			rightMotor.backward();
			leftMotor.forward();
		} else if(degrees < 0) {
			
			rightMotor.forward();
			leftMotor.backward();
		}
		
		int lT = leftMotor.getTachoCount();
		int rT = rightMotor.getTachoCount();
		while(true) {
			if(Math.abs(rightMotor.getTachoCount()-rT)+
					Math.abs(leftMotor.getTachoCount()-lT)>=11/3f*Math.abs(degrees))  {
				Sound.beep();
				LCD.clear();
				LCD.drawString(""+leftMotor.getTachoCount(), 0, 1);
				LCD.drawString(""+rightMotor.getTachoCount(), 0, 2);
				break;
			}
			Thread.sleep(50);
		}
		
		rightMotor.forward();	
		leftMotor.forward();
	}
	
	private static void turn2(int degrees) throws InterruptedException {		
		if(degrees > 0) {
			rightMotor.backward();
			leftMotor.forward();
		} else if(degrees < 0) {
			rightMotor.forward();
			leftMotor.backward();
		}
		
		int tmpLeft  = leftMotor.getTachoCount();
		int tmpRight = rightMotor.getTachoCount();
		while(true) {
//			if(Math.abs(rightMotor.getTachoCount())+
//					Math.abs(leftMotor.getTachoCount())>=11/3f*Math.abs(degrees))  {
//				break;
			if(Math.abs(leftMotor.getTachoCount() - tmpLeft) + 
					Math.abs(rightMotor.getTachoCount() - tmpRight) >= 4*Math.abs(degrees)) {
				break;
			}
			Thread.sleep(50);
		}
		
		rightMotor.forward();	
		leftMotor.forward();
	}

}
